using System;
using System.Collections.Generic;
using System.Drawing;
using System.Drawing.Drawing2D;

namespace Diversity.Robotics.Navigation
{
    /// <summary>
    /// A definition of the robot, its sensors, its navigator and motion model.
    /// </summary>
    public class Robot
    {
        #region Public and Private Members
        private Pose2D _pose2DCurrent;
        private Pose2D _pose2DLast;

        /// <summary>
        /// A copy of the robot's last state.
        /// </summary>
        public Pose2D Pose2DLast
        {
            get { return _pose2DLast; }
            set { _pose2DLast = value; }
        }

        private Velocity _velocity;

        private double _width;
        private double _length;
        private List<DistanceSensor> _distanceSensors;
        private Map _globalMap;
        private Map _localMap;
        private IMotionModel _motionModel;
        private double _obstacleClearance = 0;
        private double _obstacleLookAhead = 200;
        private double _obstacleAngleStep = 0.05;
        private double _maxSpeed;
        private double _maxTurn;
        private double _maxAccel;
        private double _maxTurnAccel;

        private IAvoidObstacles _navigator;

        /// <summary>
        /// The X, Y, Theta of the robot.
        /// </summary>
        public Pose2D Pose2DCurrent {
            get { return _pose2DCurrent; }
            set { _pose2DCurrent = value; }
        }

        /// <summary>
        /// The latest velocity.
        /// </summary>
        public Velocity Velocity
        {
            get { return _velocity; }
            set { _velocity = value; }
        }

        /// <summary>
        /// The width of the robot.
        /// </summary>
        public double Width
        {
            get { return _width; }
            set { _width = value; }
        }

        /// <summary>
        /// The length of the robot.
        /// </summary>
        public double Length
        {
            get { return _length; }
            set { _length = value; }
        }

        /// <summary>
        /// The maximum speed.
        /// </summary>
        public double MaxSpeed
        {
            get { return _maxSpeed; }
            set { _maxSpeed = value; }
        }

        /// <summary>
        /// The maximum acceleration.
        /// </summary>
        public double MaxAccel
        {
            get { return _maxAccel; }
            set { _maxAccel = value; }
        }

        /// <summary>
        /// The maximum steering angle.
        /// </summary>
        public double MaxTurn
        {
            get { return _maxTurn; }
            set { _maxTurn = value; }
        }

        /// <summary>
        /// The maximum angular acceleration.
        /// </summary>
        public double MaxTurnAccel
        {
            get { return _maxTurnAccel; }
            set { _maxTurnAccel = value; }
        }

        // TODO move these out of here
        /// <summary>
        /// DEPRECIATED How close the robot is allowed to come to obstacles.
        /// </summary>
        public double ObstacleClearance
        {
            get { return _obstacleClearance; }
            set { _obstacleClearance = value; }
        }

        /// <summary>
        /// DEPRECIATED
        /// </summary>
        public double ObstacleLookAhead
        {
            get { return _obstacleLookAhead; }
            set { _obstacleLookAhead = value; }
        }

        /// <summary>
        /// DEPRECIATED
        /// </summary>
        public double ObstacleAngleStep
        {
            get { return _obstacleAngleStep; }
            set { _obstacleAngleStep = value; }
        }

        /// <summary>
        /// A list of all the distance sensors attached to the robot.
        /// </summary>
        public List<DistanceSensor> DistanceSensors
        {
            get { return _distanceSensors; }
            set { _distanceSensors = value; }
        }

        /// <summary>
        /// Used in simulation as a model of the world.
        /// Defines the size and resolution of the world.
        /// </summary>
        public Map GlobalMap
        {
            get { return _globalMap; }
            set { _globalMap = value; }
        }

        /// <summary>
        /// The map produced from raw odometry.
        /// </summary>
        public Map LocalMap
        {
            get { return _localMap; }
            set { _localMap = value; }
        }

        /// <summary>
        /// A model of the uncertainty in movement.
        /// </summary>
        public IMotionModel MotionModel
        {
            get { return _motionModel; }
            set { _motionModel = value; }
        }

        /// <summary>
        /// A function which can alter a commanded velocity
        /// based on the map.
        /// </summary>
        public IAvoidObstacles Navigator
        {
            get { return _navigator; }
            set { _navigator = value; }
        }
        #endregion

        /// <summary>
        /// Creates a robot.
        /// </summary>
        /// <param name="pose2D">Where the robot is in the map.</param>
        /// <param name="width">The width in cm of a square robot.</param>
        /// <param name="length">The length in cm of a square robot</param>
        /// <param name="globalMap">A global map - could be blank.</param>
        public Robot(Pose2D pose2D, double width, double length, Map globalMap)
        {
            _pose2DCurrent = pose2D;
            _pose2DLast = pose2D;
            _width = width;
            _length = length;
            _distanceSensors = new List<DistanceSensor>();
            _globalMap = globalMap;
            _localMap = new Map(_globalMap.Width, _globalMap.Height, _globalMap.Threshold, _globalMap.Scale);
            _velocity = new Velocity(0, 0);
            _navigator = (IAvoidObstacles) new NullNavigator();
        }

        /// <summary>
        /// Sets the motion limits.
        /// </summary>
        /// <param name="maxSpeed">The maximum speed allowed.</param>
        /// <param name="maxTurn">The maximum turn of steering.</param>
        /// <param name="maxAccel">The maximum acceleration.</param>
        /// <param name="maxTurnAccel">The maximum angular acceleration.</param>
        public void SetMotionParameters(double maxSpeed, double maxTurn, double maxAccel, double maxTurnAccel)
        {
            _maxSpeed = maxSpeed;
            _maxTurn = maxTurn;
            _maxAccel = maxAccel;
            _maxTurnAccel = maxTurnAccel;
        }

        /// <summary>
        /// Attaches a distance sensor to the robot.
        /// </summary>
        /// <param name="distanceSensor"></param>
        public void AddDistanceSensor(DistanceSensor distanceSensor) {
            _distanceSensors.Add(distanceSensor);
        }

        /// <summary>
        /// Creates readings for each sensor from the given map
        /// </summary>
        /// <param name="map">The occupancy grid to raycast.</param>
        /// <param name="pose2D">Where the robot is.</param>
        /// <param name="percentNoise">The ammount of noise to add.</param>
        /// <returns></returns>
        public virtual DistanceReading[] GetDistanceReadings( Map map, Pose2D pose2D, double percentNoise)
        {
            List<DistanceReading> readings = new List<DistanceReading>();
            foreach(DistanceSensor distanceSensor in _distanceSensors) {
                readings.Add(distanceSensor.GetReading(map, pose2D, percentNoise));
            }
            return readings.ToArray();
        }

        /// <summary>
        /// Gives a score relating to the similarity of two sets of distance readings by using each sensor's beam model.
        /// </summary>
        /// <param name="hitDistances">The 'true' readings.</param>
        /// <param name="testDistances">The readings to test to test.</param>
        /// <returns>A number between zero and one representing probability.</returns>
        public double CompareDistanceReadings(DistanceReading[] hitDistances, DistanceReading[] testDistances)
        {
            double prob = 1; 
            for (int i = 0; i < hitDistances.Length; i++)
            {
                int hitDistance = (int) Math.Round(hitDistances[i].Distance);
                int testDistance = (int) Math.Round(testDistances[i].Distance);
                if (hitDistance != 0)
                {
                    double newProb = hitDistances[i].Beam.GetProbability(hitDistance, testDistance);
                    if (newProb == 0)
                    {
                        Console.Out.WriteLine(hitDistance + " " + testDistance + " zero prob");
                    }
                    prob *= (newProb);// (prob * newProb) / (newProb + prob); // was *= newProb (much stronger)
                }
            }
          //  Console.Out.WriteLine("prob: " + prob);
            return prob;
        }

        /// <summary>
        /// Passes the velocity to the IAvoidObstacles interface, passes
        /// the result into the motion model and moves the robot.
        /// </summary>
        /// <param name="vel">The desired velocity.</param>
        public virtual void Move(Velocity vel)
        {
            TimeSpan t = new TimeSpan(0, 0, 1);
            Velocity resultVelocity = Navigator.GetVelocity(_globalMap, _pose2DCurrent, vel);
            _velocity = resultVelocity;
            _motionModel.PrepareForSampling(_velocity, t);
            _pose2DCurrent = _motionModel.GetPerfectSample(_pose2DCurrent);
        }

        public virtual void Move(Pose2D state)
        {
            _motionModel.PrepareForSampling(state,_pose2DLast);
            _pose2DLast = (Pose2D) state.Clone();
            _pose2DCurrent = _motionModel.GetPerfectSample(state);
        }

        /*
        public void Move(Velocity vel, out Point[] points)
        {
           vel =  Navigator.GetVelocity(GlobalMap, _state, vel);
            TimeSpan t = new TimeSpan(0,0,1);
            _motionModel.PrepareForSampling(vel, t);

            //Point[] points = new Point[0];
            vel = GetSafeVelocity(_state, vel, out points);
            if (points.Length > 0)
            {
                _state = _motionModel.GetPerfectSample(_state, vel, t);
                _motionModel.PrepareForSampling(vel,t);
                _velocity = vel;
            } else
            {
                Console.Out.WriteLine("Could not move");

            }
            /*
            DifferentialDriveVelocity tmpVelocity = vel;
            DifferentialDriveState tmpState = new DifferentialDriveState(0,0,0);
           
            int direction = 1;
            int strength = 1;
            double originalDirection = _velocity.Rotation;

            int attempts = 1;
            while (attempts< 100) // ha!
            {
                attempts++;
                tmpState = _motionModel.GetPerfectSample(_state, tmpVelocity,t);
                if (IsValidState(tmpState))
                {
                    _motionModel.PrepareForSampling(tmpVelocity,t );
                    break;
                }
                else
                {
                    double newRotation = originalDirection + (direction * strength * (Math.PI / 30));
                    if (direction == 1)
                    {
                        direction = -1;
                    }
                    else
                    {
                        strength++;
                        direction = 1;
                    }
                    tmpVelocity = new DifferentialDriveVelocity(tmpVelocity.Translation, newRotation);
                }
            }
            _state = tmpState;
            _velocity = tmpVelocity;
             * */
            /*
        }
    */

        /// <summary>
        /// Tests the Global Map for overlap between occupied cells and the robot's safe outline
        /// </summary>
        /// <param name="testState">The state of the robot</param>
        /// <returns>True if there is no overlap</returns>
        public bool IsValidState(Pose2D testState)
        {
            return IsValidState(_globalMap, testState);
        }

        /// <summary>
        /// Tests a given map for overlap between occupied cells and the robot's safe outline.
        /// </summary>
        /// <param name="m">The map you want to test.</param>
        /// <param name="testState">The state of the robot.</param>
        /// <returns>True if there is no overlap.</returns>
        public bool IsValidState(Map m, Pose2D testState)
        {
            if (m.IsOccupied(testState.ToPoint()))
            {
                return false;
            }
            else
            {
                List<Point> corners = GetSafeShapeAsPoints(testState);
                bool safe = true;
                corners.ForEach(delegate(Point p)
                    {
                        if (m.IsOccupied(p))
                        {
                            safe = false;
                        }
                    });
                return safe;
            }
        }

        #region ALL THIS SHOULD MOVE INTO ANOTHER CLASS
        /// <summary>
        /// DEPRECIATED
        /// This needs moving into a seperate INavigate class
        /// </summary>
        /// <param name="state"></param>
        /// <param name="desiredVelocity"></param>
        /// <param name="points"></param>
        /// <returns></returns>
        public Velocity GetSafeVelocity(Pose2D state, Velocity desiredVelocity, out Point[] points)
        {
            int numsteps = (int) Math.Floor(_maxTurnAccel/_obstacleAngleStep);
            bool success = false;
            points = new Point[0];

            double deceleration;
            // ramp back up to max speed
            if (desiredVelocity.Translation < _maxSpeed)
            {
                deceleration = _maxSpeed - desiredVelocity.Translation;
            } else
            {
                deceleration = 0;
            }
            while (deceleration >= -_maxAccel)
            {
                Velocity testAccel = new Velocity(deceleration, 0);

                int direction = 1;
                
                for (int i = 0; i <= numsteps;)
                {
                    if (!IsSafeVelocityChange(state, desiredVelocity, testAccel, out points))
                    {
                        if (direction == 1)
                        {
                            direction = -1;
                            if (i == 0)
                            {
                                i++;
                            }
                        }
                        else
                        {
                            direction = 1;
                            i++;
                        }
                        testAccel.Rotation = i*direction*_obstacleAngleStep;
                    }
                    else
                    {
                        //Console.Out.WriteLine("Success with " + vel.ToString());
                        success = true;
                        break;
                    }
                }

                if (success)
                {
                    Velocity resultVel =  new Velocity(desiredVelocity.Translation + testAccel.Translation,
                                                      desiredVelocity.Rotation + testAccel.Rotation);
                    resultVel.CleanRotation();
                    return resultVel;
                       
                }
                else
                {
                    deceleration -= 5;
                    Console.Out.WriteLine("Decelerating");
                }
            }
            Console.Out.WriteLine("Could not find a safe velocity");
            return new Velocity(0, 0); // stop !
        }

        /// <summary>
        /// DEPRECIATED .... needs to move into another class
        /// </summary>
        /// <param name="currentState"></param>
        /// <param name="currentVelocity"></param>
        /// <param name="testAccel"></param>
        /// <param name="points"></param>
        /// <returns></returns>
        public bool IsSafeVelocityChange(Pose2D currentState, Velocity currentVelocity, Velocity testAccel, out Point[] points)
        {
            List<Point> pointList = new List<Point>();
            
           // Console.Out.WriteLine("testing " + testVel.ToString());
            Velocity vel =
                new Velocity(currentVelocity.Translation + testAccel.Translation,
                                              currentVelocity.Rotation + testAccel.Rotation);

            // check limits
            if (vel.Rotation > _maxTurn)
            {
                Console.Out.WriteLine("Excesive rotation");
                points = pointList.ToArray();
                return false;
            }
            else if (vel.Rotation < -_maxTurn)
            {
                Console.Out.WriteLine("Excesive rotation");
                points = pointList.ToArray();
                return false;
            }

            if (vel.Translation > _maxSpeed)
            {
                Console.Out.WriteLine("Limiting speed");
                vel.Translation = _maxSpeed;
            }

            TimeSpan timeStep = new TimeSpan(0, 0, 0, 0, 100);
            double currentPathLength = 0;
            int turnSteps = 1;
            int step = 0;
            int maxTurnSteps = (int)Math.Floor(_obstacleLookAhead / (vel.Translation * timeStep.TotalSeconds));
           // Console.Out.WriteLine("Max turn steps is " + maxTurnSteps + " look ahead: " + _obstacleLookAhead + " step trans " + (vel.Translation * timeStep.TotalSeconds));
            Pose2D testState = (Pose2D)currentState.Clone();
            while (currentPathLength <= _obstacleLookAhead)
            {
                //Console.Out.WriteLine("CurrentPathLength " + currentPathLength);
                if (IsValidState(testState))
                {
                    if (vel.Translation == 0)
                    {
                        points = pointList.ToArray();
                        Console.Out.WriteLine("Translation was zero");
                        return false;
                    }
                    Pose2D lastState = (Pose2D) testState.Clone();
                    testState = ((VelocityMotionModel)_motionModel).GetPerfectSample(testState, vel, timeStep);
                    pointList.Add(testState.ToPoint());
                    // Console.Out.WriteLine("Path+= " + testState.ToString());
                    currentPathLength += testState.DistanceTo(lastState);

                    if (step++ < turnSteps)
                    {
                        // continue turning
                        vel.RotateWithinLimit(testAccel.Rotation * timeStep.TotalSeconds, _maxTurn);
                       
                       // Console.Out.WriteLine("Step:" + step + " of " + turnSteps + " at rot of " + vel.Rotation);
                    }
                    else
                    {
                        vel.Dampen(_maxTurnAccel);
                    }
                }
                else
                {
                    if (turnSteps >= maxTurnSteps)
                    {
                        //Console.Out.WriteLine("Invalid path for " + testAccel.ToString());
                        pointList.Add(currentState.ToPoint());
                        points = pointList.ToArray();
                        return false;
                    } else
                    {
                        pointList.Clear();
                        vel = new Velocity(currentVelocity.Translation + testAccel.Translation,
                                              currentVelocity.Rotation + testAccel.Rotation);
                        testState = (Pose2D)currentState.Clone();
                        currentPathLength = 0;
                     //   Console.Out.WriteLine("Cleared");
                        step = 1;
                        turnSteps++;
                    }
                }
            }
            points = pointList.ToArray();
            return true;
        }

        /// <summary>
        /// DEPRECIATED .... needs to move into another class
        /// </summary>
        /// <param name="vel"></param>
        /// <returns></returns>
        public Velocity GoStraighter(Velocity vel)
        {
            if (vel.Rotation == 0)
            {
                return vel;
            }
            if (vel.Rotation > 0)
            {
                vel.Rotation -= 0.05;
                if (vel.Rotation < 0)
                {
                    vel.Rotation = 0;
                }
            }
            else if (vel.Rotation < 0)
            {
                vel.Rotation += 0.05;
                if (vel.Rotation > 0)
                {
                    vel.Rotation = 0;
                }
            }
            return vel;
        }

        /*
        public bool IsSafeVelocity(DifferentialDriveState state, DifferentialDriveVelocity testVel, double lookAhead)
        {
            TimeSpan timeStep = new TimeSpan(0, 0, 0, 0, 100);
            double currentPathLength = 0;
            DifferentialDriveState testState = (DifferentialDriveState) state.Clone();
            DifferentialDriveVelocity vel = (DifferentialDriveVelocity)testVel.Clone();
            while (currentPathLength <= lookAhead)
            {
                if (IsValidState(testState))
                {
                    if (vel.Translation == 0)
                    {
                        return true;
                    }
                    DifferentialDriveState lastState = (DifferentialDriveState) testState.Clone();
                    testState = _motionModel.GetPerfectSample(testState, vel, timeStep);
                    currentPathLength += testState.GetDistanceTo(lastState);
                } 
                else
                {
                    return false;
                }
            }
            return true;
        }
         * */
        #endregion

        /// <summary>
        /// Returns a list of the robot's corners.
        /// </summary>
        /// <param name="state">The robot's state.</param>
        /// <returns>The corners.</returns>
        public List<Point> GetShapeAsPoints(Pose2D state)
        {
            double cosTheta = Math.Cos(state.Theta);
            double sinTheta = Math.Sin(state.Theta);
            double yDist = _width/2;
            double xDist = _length/2;

            double dx =  state.X + (cosTheta * xDist - sinTheta * yDist);
            double dy = state.Y + (sinTheta * xDist + cosTheta * yDist);

            List<Point> points = new List<Point>();
            points.Add(new Point((int)Math.Round(dx / _globalMap.Scale), (int)Math.Round(dy / _globalMap.Scale)));

            dx = state.X + (cosTheta * xDist - sinTheta * -yDist);
            dy = state.Y + (sinTheta * xDist + cosTheta * -yDist);
            points.Add(new Point((int)Math.Round(dx / _globalMap.Scale), (int)Math.Round(dy / _globalMap.Scale)));

            dx = state.X + ( cosTheta * -xDist - sinTheta * -yDist);
            dy = state.Y + (sinTheta * -xDist + cosTheta * -yDist);
            points.Add(new Point((int)Math.Round(dx / _globalMap.Scale), (int)Math.Round(dy / _globalMap.Scale)));

            dx = state.X + ( cosTheta * -xDist - sinTheta * yDist);
            dy = state.Y + ( sinTheta * -xDist + cosTheta * yDist);
            points.Add(new Point((int)Math.Round(dx / _globalMap.Scale), (int)Math.Round(dy / _globalMap.Scale)));

            return points;
        }

        /// <summary>
        /// Returns a list of the robot's corners plus the clearance.
        /// </summary>
        /// <param name="state">The robot's state.</param>
        /// <returns>The corners.</returns>
        public List<Point> GetSafeShapeAsPoints(Pose2D state)
        {
            double cosTheta = Math.Cos(state.Theta);
            double sinTheta = Math.Sin(state.Theta);
            double yDist = (_width / 2) + _obstacleClearance;
            double xDist = (_length / 2) + _obstacleClearance;

            double dx = state.X + (cosTheta * xDist - sinTheta * yDist);
            double dy = state.Y + (sinTheta * xDist + cosTheta * yDist);

            List<Point> points = new List<Point>();
            points.Add(new Point((int)Math.Round(dx / _globalMap.Scale), (int)Math.Round(dy / _globalMap.Scale)));

            dx = state.X + (cosTheta * xDist - sinTheta * -yDist);
            dy = state.Y + (sinTheta * xDist + cosTheta * -yDist);
            points.Add(new Point((int)Math.Round(dx / _globalMap.Scale), (int)Math.Round(dy / _globalMap.Scale)));

            dx = state.X + (cosTheta * -xDist - sinTheta * -yDist);
            dy = state.Y + (sinTheta * -xDist + cosTheta * -yDist);
            points.Add(new Point((int)Math.Round(dx / _globalMap.Scale), (int)Math.Round(dy / _globalMap.Scale)));

            dx = state.X + (cosTheta * -xDist - sinTheta * yDist);
            dy = state.Y + (sinTheta * -xDist + cosTheta * yDist);
            points.Add(new Point((int)Math.Round(dx / _globalMap.Scale), (int)Math.Round(dy / _globalMap.Scale)));

            return points;
        }

    }
}
